//
// Created by An on 2025/7/7.
//
#include "motor.h"
#include "gpio.h"
#include "tim.h"


static void Motor_Init(Motor_t *motor,
                       TIM_HandleTypeDef *htim_encoder,
                       TIM_HandleTypeDef *htim_pwm,
                       uint32_t pwm_channel,
                       GPIO_TypeDef *dir_port1, uint16_t dir_pin1,
                       GPIO_TypeDef *dir_port2, uint16_t dir_pin2)
{
  motor->htim_encoder = htim_encoder;
  motor->htim_pwm = htim_pwm;
  motor->pwm_channel = pwm_channel;

  motor->dir_port1 = dir_port1;
  motor->dir_pin1 = dir_pin1;
  motor->dir_port2 = dir_port2;
  motor->dir_pin2 = dir_pin2;

  motor->last_encoder_value = __HAL_TIM_GET_COUNTER(htim_encoder);
  motor->encoder_count = 0;

  motor->target_speed = 0;
  motor->current_speed = 0;
  motor->target_position = 0;

  // === PID 初始化（只做速度环） ===
  PID_Init(&motor->speed_pid, 50.0f, 5.0f, 0.0f,-10000,10000);              // 可调参数
  PID_Init(&motor->position_pid, 5.0f, 0.0f, 10.0f,-3000,3000);

  HAL_TIM_Encoder_Start(htim_encoder, TIM_CHANNEL_ALL);
  HAL_TIM_PWM_Start(htim_pwm, pwm_channel);
}


void Motors_Init(void)
{
  Motor_Init(&motor_A, &htim1, &htim5, TIM_CHANNEL_1, AIN1_GPIO_Port, AIN1_Pin, AIN2_GPIO_Port, AIN2_Pin);
  Motor_Init(&motor_B, &htim2, &htim5, TIM_CHANNEL_2, BIN1_GPIO_Port, BIN1_Pin, BIN2_GPIO_Port, BIN2_Pin);
  Motor_Init(&motor_C, &htim3, &htim5, TIM_CHANNEL_3, CIN1_GPIO_Port, CIN1_Pin, CIN2_GPIO_Port, CIN2_Pin);
  Motor_Init(&motor_D, &htim4, &htim5, TIM_CHANNEL_4, DIN1_GPIO_Port, DIN1_Pin, DIN2_GPIO_Port, DIN2_Pin);

  PID_Init(&motor_A.speed_pid, 50.0f, 5.0f, 0.0f,-10000,10000);
  PID_Init(&motor_B.speed_pid, 50.0f, 5.0f, 0.0f,-10000,10000);
  PID_Init(&motor_C.speed_pid, 50.0f, 5.0f, 0.0f,-10000,10000);
  PID_Init(&motor_D.speed_pid, 50.0f, 5.0f, 0.0f,-10000,10000);

}

void Motor_SetPWM(Motor_t *motor, int32_t pwm_val)
{
  // === 获取 PWM 最大值（自动） ===
  uint32_t pwm_max = motor->htim_pwm->Init.Period;

  // === 确定方向 ===
  if (pwm_val > 0)
  {
    HAL_GPIO_WritePin(motor->dir_port1, motor->dir_pin1, GPIO_PIN_SET);
    HAL_GPIO_WritePin(motor->dir_port2, motor->dir_pin2, GPIO_PIN_RESET);
  }
  else if (pwm_val < 0)
  {
    HAL_GPIO_WritePin(motor->dir_port1, motor->dir_pin1, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(motor->dir_port2, motor->dir_pin2, GPIO_PIN_SET);
    pwm_val = -pwm_val;  // 取绝对值
  }
  else  // 停止
  {
    HAL_GPIO_WritePin(motor->dir_port1, motor->dir_pin1, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(motor->dir_port2, motor->dir_pin2, GPIO_PIN_RESET);
    pwm_val = 0;
  }

  // === 限幅 ===
  if ((uint32_t)pwm_val > pwm_max)
    pwm_val = pwm_max;

  // === 设置 PWM 占空比 ===
  __HAL_TIM_SET_COMPARE(motor->htim_pwm, motor->pwm_channel, pwm_val);
}

void Motor_UpdateEncoder(Motor_t *motor)
{
  uint16_t current_value = __HAL_TIM_GET_COUNTER(motor->htim_encoder);
  int16_t delta = (int16_t)(current_value - motor->last_encoder_value);  // 自动处理溢出

  motor->encoder_count += delta;
  motor->last_encoder_value = current_value;
}

void Motor_EstimateSpeed(Motor_t *motor, float delta_time)
{
  uint16_t now = __HAL_TIM_GET_COUNTER(motor->htim_encoder);
  int16_t delta = (int16_t)(now - motor->last_encoder_value);
  motor->last_encoder_value = now;
  motor->encoder_count += delta;

  // 原始速度估计（mm/s）
  const float mm_per_pulse = 3.1416f * 70.0f / 780.0f;
  float raw_speed = delta * mm_per_pulse / delta_time;

  // 一阶滤波
  motor->current_speed = 0.7f * motor->current_speed + 0.3f * raw_speed;
}

float GetRPM(const Motor_t *motor)
{
  const float wheel_circumference_mm = 3.1416f * 70.0f;  // 轮子周长，单位mm
  return (motor->current_speed / wheel_circumference_mm) * 60.0f;
}

void Motor_SpeedControl(Motor_t *motor)
{
  // 1. 计算 PID 输出
  float control = PID_Compute(&motor->speed_pid,
                              (float)motor->target_speed,
                              (float)motor->current_speed);

  // 2. 输出控制值给 PWM（强制转为整数）
  Motor_SetPWM(motor, (int32_t)control);
}

float Motor_GetPosition(Motor_t *motor)
{
  const float mm_per_pulse = 3.1416f * 70.0f / 780.0f;  // 轮径70mm，编码器780脉冲/圈
  return motor->encoder_count * mm_per_pulse;
}

void Motor_PositionControl(Motor_t *motor)
{
  float current_pos = Motor_GetPosition(motor);
  float pos_error = (float)motor->target_position - current_pos;

  // PID输出的单位仍然是 mm/s，供速度环使用
  float target_speed = PID_Compute(&motor->position_pid, 0.0f, -pos_error);

  motor->target_speed = (int32_t)target_speed;
}
